﻿using System;
using System.Collections.Generic;
using System.Drawing;

namespace ekf_slam_simulator
{
     class Space
     {
          #region methods of space
          public Space()
          {
               robot.space = this;
          }

          public static void print(string s)
          {
               Console.Out.WriteLine(s);
          }


          public double gaussianNoise()
          {
               return gaussianNoise(0, 1);
          }

          public double gaussianNoise(double mean, double variance)
          {
               //gaussian random using Box-Muller transform
               double u1 = rand.NextDouble();
               double u2 = rand.NextDouble();
               return mean + Math.Sqrt(variance) * Math.Sqrt(-2 * Math.Log(u1)) * Math.Cos(2 * Math.PI * u2);
          }



          #endregion

          #region robot
          public class Robot
          {
               #region methods of robot

               public Robot()
               {
                    resetSLAM();
               }
               public void resetSLAM()
               {
                    updateX();
                    Coordinates c;
                    c.angle = 0;
                    c.location = new PointF(0, 0);
                    updateA(c);
                    P = A;
               }

               public void updateX()
               {
                    int num = 3 + (2 * landmark_est.Count);
                    X = new Matrix(num, 1);
                    X[0] = location_est_prec.X;
                    X[1] = location_est_prec.Y;
                    X[2] = angle_est;
                    LandmarkList.Enumerator e = landmark_est.GetEnumerator();
                    for (int i = 0; i < 2 * landmark_est.Count; i += 2)
                    {
                         e.MoveNext();
                         Point p = e.Current.loc;
                         X[3 + i] = p.X;
                         X[3 + 1 + i] = p.Y;

                    }
                    X.print();
               }

               private void updateX(Coordinates c)
               {
                    int num = 3 + (2 * landmark_est.Count);
                    X = new Matrix(num, 1);
                    X[0] = location_est_prec.X + c.location.X;
                    X[1] = location_est_prec.Y + c.location.Y;
                    X[2] = angle_est + +c.angle;
                    LandmarkList.Enumerator e = landmark_est.GetEnumerator();
                    for (int i = 0; i < 2 * landmark_est.Count; i += 2)
                    {
                         e.MoveNext();
                         Point p = e.Current.loc;
                         X[3 + i] = p.X;
                         X[3 + 1 + i] = p.Y;

                    }
                    X.print();
               }

               private void makeJxr(Coordinates c)
               {
                    Jxr = new Matrix();
                    double dx = c.location.X;
                    double dy = c.location.Y;
                    double[,] tmp = { { 1, 0, -dy },
                                      { 0, 1, dx } };
                    Jxr.set(tmp);
               }

               private void makeJz(Landmark l)
               {
                    Jz = new Matrix();
                    Point subPoint = l.loc.sub(location_est);
                    double dt = subPoint.len();
                    if (dt == 0)
                         dt = 0.01;
                    double dx = subPoint.X;
                    double dy = subPoint.Y;

                    double[,] tmp = { { dx/dt, -dy},
                                      { dy/dt, dx} };
                    Jz.set(tmp);
               }

               private void updateQ(Coordinates c)
               {
                    Matrix W = new Matrix(3, 1);
                    W[0] = c.location.X;
                    W[1] = c.location.Y;
                    W[2] = c.angle;
                    Matrix C = new Matrix();
                    double odometry_ratio = 0.1 * odoNR * c.location.len();
                    C[0] = odometry_ratio;
                    Q = W * C * W.transpose();
               }

               private void updateA(Coordinates c)
               {
                    double dx = c.location.X;
                    double dy = c.location.Y;
                    A = new Matrix();
                    double[,] tmp = { { 1, 0, -dy },
                                       { 0, 1,dx },
                                       { 0, 0, 1 } };
                    A.set(tmp);
               }

               private void updateR(double range, double bearing)
               {
                    double c = 0.5;
                    double d = 0.5;
                    double [,] tmp = {{range * c, 0},{0, bearing * d}};
                    R = new Matrix(tmp);
               }

               private void updateP()
               {
                    Matrix Psmall = P.getSubMatrix(3, 3);
                    Psmall = (A * Psmall * A) + Q;
                    P.setSubMatrix(Psmall);
                    int max = P.n;
                    for (int i = 0; i < (P.n - 3) / 2; i++)
                    {
                         Matrix Pri = P.getSubMatrix(3, 2,new Point(0, 3 + (i * 2)));
                         Pri = A * Pri;
                         P.setSubMatrix(0, 3 + (i * 2), Pri);
                    }
               }

               private void enlargeP(LandmarkList land, Coordinates c)
               {
                    Matrix tmp = P;
                    int mp = tmp.m;
                    int np = tmp.n;
                    P = new Matrix(tmp.m + 2 * land.Count, tmp.n + 2 * land.Count);
                    P.setSubMatrix(tmp);
                    int i = 0;
                    foreach (Landmark l in land)
                    {
                         Coordinates coor;
                         Point cprim = new Point((int)c.location.X, (int)c.location.Y);
                         coor.location = location_est.add(cprim);
                         coor.angle = angle_est + c.angle;
                         makeJxr(coor);
                         makeJz(l);
                         double range = l.loc.dist(location_est.add(cprim));
                         double bearing = l.loc.angle() - angle_est + c.angle;
                         updateR(range, bearing);
                         Matrix Pnn = Jxr * P.getSubMatrix(3, 3) * Jxr.transpose() + Jz * R * Jz.transpose();
                         P.setSubMatrix(mp + (2 * i), np + (2 * i), Pnn);
                         Matrix Prn = A * Jxr.transpose();
                         Matrix Pnr = Prn.transpose();
                         P.setSubMatrix(0, np + (2 * i), Prn);
                         P.setSubMatrix(mp + (2 * i), 0, Pnr);
                         for (int j = 0; j < ((mp - 3) / 2); j++)
                         {
                              Matrix Pri = P.getSubMatrix(2, 3, new Point(0, 3 + (2 * j)));
                              Matrix Pni = Jxr * Pri.transpose();
                              P.setSubMatrix(mp + (2 * i), 3 + (2 * j), Pni);
                              P.setSubMatrix(3 + (2 * j), np + (2 * i), Pni.transpose());
                         }
                         i++;
                    }



               }

               public void doKalmanFilterMagic()
               {    
                    //Update current state using odometry data
                    Coordinates c = readOdometry();
                    double dx = c.location.X;
                    double dy = c.location.Y;
                    updateX(c);
                    updateA(c);
                    updateQ(c);
                    updateP();
                    
                    //Update state from re-observed landmarks
                    LandmarkList[] container = reobserve();
                    LandmarkList land = container[0];
                    LandmarkList newLand = container[1];
                    foreach (Landmark l in land)
                    {
                         int i = landmark_est.getIdx(l.id);
                         //data association
                         PointF tmpL = (PointF)l.loc;
                         PointF locmov = location_precise;
                         PointF tmp1 = tmpL.sub(locmov);
                         double range1 = tmp1.len();
                         double bearing1 = tmp1.angle();
                         Matrix z = new Matrix(2,1);
                         z[0] = range1;
                         z[1] = bearing1;
                        
                         //odometry update
                         double lx = X[3 + (2 * i)];
                         double ly = X[4 + (2 * i)];
                         PointF tmpL2 = new PointF((float)lx, (float)ly);
                         PointF locmov2 = location_est_prec.add(c.location);
                         PointF tmp2 = tmpL2.sub(locmov2);
                         double range = tmp2.len();
                         double bearing = tmp2.angle();
                         Matrix h = new Matrix(2,1);
                         h[0] = range;
                         h[1] = bearing;


                         double [,] vals = new double[2, 3];
                         vals[0, 0] = (tmp1.X) / range;
                         vals[0, 1] = (tmp1.Y) / range;
                         vals[0, 2] = 0;
                         vals[1, 0] = (-tmp1.Y) / (range * range);
                         vals[1, 1] = (-tmp1.X) / (range * range);
                         vals[1, 2] = -1;
                         double [,] vals2 = new double[2, 2];
                         vals2[0, 0] = -(tmp1.X) / range;
                         vals2[0, 1] = -(tmp1.Y) / range;
                         vals2[1, 0] = tmp1.Y / (range * range);
                         vals2[1, 1] = tmp1.X / (range * range);
                         z[0] = h[0] = 0;
                         z[1] = h[1] = 0;

                         H = new Matrix(2, 3 + 2 * landmark_est.Count);
                         Matrix subM1 = new Matrix(vals);
                         Matrix subM2 = new Matrix(vals2);

                         H.setSubMatrix(subM1);
                         H.setSubMatrix(0, 3 + (2 * i),subM2);

                         updateR(range, bearing);
                         double [,] tmpmat = { { 1, 0 }, { 0, 1 } };
                         Matrix V = new Matrix(tmpmat);


                         Matrix S = (H * P * H.transpose()) + (V * R * V.transpose());         //calculate innovation covariance
                         K = P * H.transpose() * S.rev();
                         Matrix addition = (z - h);
                         X = X + K * addition;
                    }



                    
                    location_est_prec.X = (float)X[0];
                    location_est_prec.Y = (float)X[1];
                    angle_est = enlargeAngle(X[2], 0);
                    for (int i = 0; i < landmark_est.Count; i++)
                    {
                         Landmark tmp = landmark_est[i];
                         tmp.loc = new Point((int)X[3 + (2 * i)], (int)X[4 + (2 * i)]);
                         landmark_est[i] = tmp;
                    } 
                    foreach (Landmark l in newLand)
                         landmark_est.Add(l);
                    updateX();
                    enlargeP(newLand, c);
                    castPointsToDraw();

               }
               
               public LandmarkList[] reobserve()
               {
                    LandmarkList k = new LandmarkList();
                    LandmarkList n = new LandmarkList();
                    
                    LandmarkList land = observeLand();
                    foreach (Landmark l in land)
                         if (landmark_est.contains(l.id))
                              k.Add(l);
                         else
                              n.Add(l);
                    LandmarkList [] arr = {k, n};
                    return arr;
               }

               public LandmarkList observeLand()
               {
                    LandmarkList landmarks = new LandmarkList();
                    foreach (Landmark landmark in space.landmark_real)
                    {
                         if (landmark.loc.dist(location_real) <= radius)
                         {
                              double noise_ratio = 0.3 * lidNR * landmark.loc.dist(location_real) / radius;
                              Point observed = new Point(landmark.loc.X + (int)(noise_ratio * space.gaussianNoise()), landmark.loc.Y + (int)(noise_ratio * space.gaussianNoise()));
                              Landmark l;
                              l.id = landmark.id;
                              l.loc = observed;
                              landmarks.Add(l);
                         }
                    }
                    return landmarks;
               }

               public LinkedList<Point> observe()
               {
                     LinkedList<Point> landmark_tmp = new LinkedList<Point>();
                    LandmarkList landmarks = new LandmarkList();
                    foreach (Landmark landmark in space.landmark_real)
                    {
                         if (landmark.loc.dist(location_real) <= radius)
                         {
                              double noise_ratio = 0.3 * lidNR * landmark.loc.dist(location_real) / radius;
                              Point observed = new Point(landmark.loc.X + (int)(noise_ratio * space.gaussianNoise()), landmark.loc.Y + (int)(noise_ratio * space.gaussianNoise()));
                              Point dist = observed.sub(location_real);
                              observed = dist.add(location_est);
                              landmark_tmp.AddLast(observed);
                              Landmark l;
                              l.id = landmark.id;
                              l.loc = observed;
                              landmarks.Add(l);
                         }
                    }
                    if (!space.ekfOn)
                    {
                         foreach (Landmark l in landmarks)
                         {
                              int been = -1;
                              foreach (Landmark k in landmark_est)
                                   if (k.id == l.id)
                                        been = k.id;
                              if (been == -1)
                                   landmark_est.Add(l);
                              else
                                   landmark_est.SetAtId(been, l);
                         }
                    }
                    return landmark_tmp;
               }
               public LinkedList<Point> observeLaser()
               {
                    LinkedList<Point> landmark_tmp = new LinkedList<Point>();
                    LandmarkList landmarks = new LandmarkList();
                    foreach (Landmark landmark in space.landmark_real)
                    {
                         if (landmark.loc.dist(location_real) <= radius)
                         {
                              double noise_ratio = 0.3 * lidNR * landmark.loc.dist(location_real) / radius;
                              Point observed = new Point(landmark.loc.X + (int)(noise_ratio * space.gaussianNoise()), landmark.loc.Y + (int)(noise_ratio * space.gaussianNoise()));
                              Landmark l;
                              landmark_tmp.AddLast(observed);
                              l.id = landmark.id;
                              l.loc = observed;
                              landmarks.Add(l);
                         }
                    }
                    return landmark_tmp;
               }

               public void resetData()
               {
                    landmark_est = new LandmarkList();
                    angle_est = angle_real;
                    location_est_prec = location_precise;
                    resetSLAM();
                    castPointsToDraw();
               }


               //makes robot move in specified direction
               public bool follow(Point p)
               {
                    Point vector = p.sub(location_real);
                    double last_angle = 0;
                    double angle1 = vector.angle() - angle_real;           //calculates clockwise direction angle.
                    if (angle1 < 0)
                         angle1 += 360;
                    double angle2 = Math.Abs(360 - angle1);                //calculates counterclockwise direction angle.
                    double angle = Math.Min(angle1, angle2);
                    double len = vector.len();

                    

                    if (len == 0)
                         return true;                                      //checks if direction has been reached.

                    if (Math.Abs(angle) > dRotConst)                              //checks if angle is smaller then tolerance.
                    {
                         dX = 0;
                         dY = 0;
                         if (angle1 < angle2)                              //rotates robot in chosen direction with specified speed.
                              dRot = dRotConst;
                         else
                              dRot = -dRotConst;
                         angle_real += (int)dRot;
                         rotateState = 0;

                         if (angle_real >= 360)
                              angle_real -= 360;
                         else if (angle_real < 0)
                              angle_real += 360;
                         estimate();
                         return false;
                    }
                    else
                    {
                         last_angle = angle_real - angle_est;
                         if (rotateState == 0)
                         {
                              dRot = last_angle;
                              rotateState = 1;
                         }
                         else if (rotateState == 1)
                         {
                              dRot = 0;
                              rotateState = 2;
                         }
                         

                         angle_real = vector.angle();
                         dX = vector.X / len;
                         dY = vector.Y / len;
                         location_precise.X += (float)dX;                   //moves robot and calculates its precise position.
                         location_precise.Y += (float)dY;
                         castPointsToDraw();


                         //checks if destionation has been reached.
                         estimate();
                         if (((int)location_real.X != (int)p.X) || ((int)location_real.Y != (int)p.Y))
                              return false;
                         else
                              return true;
                    }
               }

               private void estimate()
               {
                    if (space.ekfOn)
                         doKalmanFilterMagic();
                    else
                    {
                         Coordinates c = readOdometry();
                         location_est_prec = location_est_prec.add(c.location);
                         castPointsToDraw();
                         angle_est += c.angle;
                    }
               }


               private Coordinates readOdometry()
               {
                    float x, y;

                    double x_noise = space.gaussianNoise();
                    double y_noise = space.gaussianNoise();
                    double noise_ratio = 0.1 * odoNR;
                    x = (float)(dX + noise_ratio * dX * x_noise);    //adds gaussian noise
                    y = (float)(dY + noise_ratio * dY * y_noise);
                    double angle = dRot + noise_ratio * 0.1 * space.gaussianNoise();

                    return new Coordinates(new PointF(x, y), angle);

               }

               private double enlargeAngle(double angle, double add)
               {
                    double ret = angle + add;
                    if (ret >= 360)
                         ret -= 360;
                    else if (ret < 0)
                         ret += 360;
                    return ret;
               }

               private void castPointsToDraw()
               {
                    location_real.X = (int)location_precise.X;        //rounds position in drawing purposes.
                    location_real.Y = (int)location_precise.Y;
                    location_est.X = (int)location_est_prec.X;        //rounds estimated position in drawing purposes.
                    location_est.Y = (int)location_est_prec.Y;
               }

               public void setLoc(Point location)
               {
                    location_real = location;
                    location_precise = new PointF((float)location.X, (float)location.Y);
               }

               public void setLocEst(Point location)
               {
                    location_est = location;
                    location_est_prec = new PointF((float)location.X, (float)location.Y);
               }

               #endregion

               #region variables or robot

               private struct Coordinates
               {
                    public Coordinates(PointF location, double angle)
                    {
                         this.location = location;
                         this.angle = angle;
                    }
                    public PointF location;
                    public Double angle;
               }

               /* overview of Matrixes:
                * X - System state
                * P - covariance matrix
                * K - Kalman gain
                * H - Jacobian of measurement model
                * A - Jacobian of prediction model
                * Jxr, Jz - SLAM specific Jacobians
                * Q - process noise
                * R - measurement noise */

               private Matrix X, P, K, H, A, Jxr, Jz, Q, R;

               
               public Point
                    location_real = new Point(),
                    location_est = new Point();

               private PointF
                    location_precise = new PointF(),
                    location_est_prec = new PointF();


               public Double
                    angle_real = Math.PI,
                    angle_est = 0d,
                    radius = 150d,
                    dRot = 3,
                    dX = 0,
                    dY = 0;

               public int rotateState = 0;
               public static double
                    dRotConst = 3;


               public Space space;

               public double odoNR = 0,
                             lidNR = 0;

               public LandmarkList landmark_est = new LandmarkList();
               #endregion
          }

          #endregion

          #region variables
          public LandmarkList landmark_real = new LandmarkList();
          public Robot robot = new Robot();
          public Random rand = new Random();
          public bool ekfOn = false;

          #endregion
     }
     #region matrix math
     public class Matrix
     {
          private double[,] arr;
          public int m, n;
          public Matrix() : this(1, 1)
          {
          }
          public Matrix(int m, int n)
          {
               this.m = m;
               this.n = n;
               arr = new Double[m, n];
               for (int i = 0; i < m; i++)
                    for (int j = 0; j < n; j++)
                         arr[i, j] = 0;
          }
          public Matrix(double[,] m)
          {
               this.set(m);
          }

          public static implicit operator double[,](Matrix m)
          {
               return m.arr;
          }

          public Matrix rev()
          {
               if (m == 2 && n == 2)
               {
                    double det = (this[0] * this[3]) - (this[1] * this[2]);
                    Matrix ret = new Matrix(2, 2);
                    ret[0] = this[3]/det;
                    ret[1] = -this[1]/det;
                    ret[2] = -this[2]/det;
                    ret[3] = this[0]/det;
                    return ret;


               }
               else
                    throw new ArgumentException("Only 2x2 Matrixes are supported.");
          }

          public static Matrix operator +(Matrix a, Matrix b)
          {
               if (a.m == b.m && a.n == b.n)
               {
                    Matrix m = new Matrix(a.m, a.n);
                    for (int i = 0; i < a.m; i++)
                         for (int j = 0; j < a.n; j++)
                              m[i, j] = a[i, j] + b[i, j];
                    return m;
               }
               else
                    throw new ArgumentException("Matrixes must be the same size");
          }

          public void print()
          {
               Console.Out.WriteLine("matrix " + m + "x" + n + "\n");

               for (int i = 0; i < m; i++)
               {
                    Console.Out.Write("|");
                    for (int j = 0; j < n; j++)
                    {
                         Console.Out.Write(arr[i, j]);
                         if (j < n - 1)
                              Console.Out.Write("\t");
                    }
                    Console.Out.Write("|\n");
               }
          }

          public static Matrix operator -(Matrix a, Matrix b)
          {
               if (a.m == b.m && a.n == b.n)
               {
                    Matrix m = new Matrix(a.m, a.n);
                    for (int i = 0; i < a.m; i++)
                         for (int j = 0; j < a.n; j++)
                              m[i, j] = a[i, j] - b[i, j];
                    return m;
               }
               else
                    throw new ArgumentException("Matrixes must be the same size");
          }

          public static Matrix operator *(Matrix a, Matrix b)
          {
               if (a.n == b.m)
               {
                    Matrix m = new Matrix(a.m, b.n);
                    for (int i = 0; i < a.m; i++)
                         for (int j = 0; j < b.n; j++)
                              for (int k = 0; k < a.n; k++)
                                   m[i, j] += a[i, k] * b[k, j];
                    return m;
               }
               else
                    throw new ArgumentException("Matrixes must agree in middle dimension");
          }

          public void set(Matrix m)
          {
               this.m = m.m;
               this.n = m.n;
               this.arr = m.arr;
          }

          public void set(double[,] m)
          {
               this.arr = m;
               this.m = m.GetLength(0);
               this.n = m.GetLength(1);
          }

          public Matrix transpose()
          {
               Matrix mat = new Matrix(this.n, this.m);
               for (int i = 0; i < this.m; i++)
                    for (int j = 0; j < this.n; j++)
                         mat[j, i] = this[i, j];
               return mat;
          }

          public void setSubMatrix(Matrix m)
          {
               setSubMatrix(0, 0, m);
          }

          public void setSubMatrix(int i, int j, Matrix m)
          {
               if (m.m + i <= this.m && m.n + j <= this.n)
               {
                    for (int k = 0; k < m.m; k++)
                         for (int l = 0; l < m.n; l++)
                              arr[i + k, j + l] = m[k, l];
               }
               else
                    throw new ArgumentException("Matrix must be smaller or the same size as base matrix.");

          }
          public Matrix getSubMatrix(int i, int j)
          {
               return getSubMatrix(i, j, new Point(0, 0));
          }


          public Matrix getSubMatrix(int i, int j, Point loc)
          {
               if (loc.X + i <= this.m && loc.Y + j <= this.n)
               {
                    Matrix ret = new Matrix(i, j);
                    for (int k = 0; k < i; k++)
                         for (int l = 0; l < j; l++)
                              ret[k, l] = arr[loc.X + k, loc.Y + l];
                    return ret;
               }
               else
                    throw new ArgumentException("Matrix must be smaller or the same size as base matrix.");



          }
          public double this[int i, int j]
          {
               get
               {
                    if (i < this.m && i >= 0 && j < this.n && j >= 0)
                         return arr[i, j];
                    else
                         throw new ArgumentOutOfRangeException();
               }
               set
               {
                    if (i < this.m && i >= 0 && j < this.n && j >= 0)
                         arr[i, j] = value;
                    else
                         throw new ArgumentOutOfRangeException();
               }
          }

          public double this[int i]
          {
               get
               {
                    if (i < this.m * this.n)
                         return arr[(int)(i / n), i % n];
                    else
                         throw new ArgumentOutOfRangeException();
               }
               set
               {
                    if (i < this.m * this.n)
                         arr[(int)(i / n), i % n] = value;
                    else
                         throw new ArgumentOutOfRangeException();
               }
          }
     }
     #endregion

     public struct Landmark
     {
          public Point loc;
          public int id;
     }

     #region Point to vector extension
     public static class PointExtension
     {
          public static PointF add(this PointF a, PointF b)
          {
               PointF c = new PointF();
               c.X = a.X + b.X;
               c.Y = a.Y + b.Y;
               return c;
          }
          public static PointF sub(this PointF p, PointF b)
          {
               return new PointF(p.X - b.X, p.Y - b.Y);
          }
          public static Double angle(this PointF p)
          {
               if (p.Y == 0)
                    if (p.X > 0)
                         return 0;
                    else
                         return 180d;
               else if (p.X == 0)
                    if (p.Y > 0)
                         return 270d;
                    else
                         return 90d;
               else
               {
                    double quater = 0;
                    double angle = 180 * Math.Asin(Math.Abs(p.Y / p.len())) / Math.PI;
                    if (p.Y > 0)
                    {
                         quater += 180;
                         if (p.X > 0)
                         {
                              quater += 180;
                              angle = -1 * angle;
                         }
                    }
                    else
                    {
                         if (p.X < 0)
                         {
                              quater += 180;
                              angle = -1 * angle;
                         }
                    }

                    return quater + angle;
               }

          }
          public static Double len(this PointF vector)
          {
               return Math.Sqrt((vector.X * vector.X) + (vector.Y * vector.Y));
          }
          public static Double dist(this PointF a, PointF b)
          {
               PointF c = a.sub(b);
               return c.len();
          }

          public static Point add(this Point a, Point b)
          {
               Point c = new Point();
               c.X = a.X + b.X;
               c.Y = a.Y + b.Y;
               return c;
          }
          public static Point sub(this Point p, Point b)
          {
               return new Point(p.X - b.X, p.Y - b.Y);
          }
          public static Double angle(this Point p)
          {
               if (p.Y == 0)
                    if (p.X > 0)
                         return 0;
                    else
                         return 180d;
               else if (p.X == 0)
                    if (p.Y > 0)
                         return 270d;
                    else
                         return 90d;
               else
               {
                    double quater = 0;
                    double angle = 180 * Math.Asin(Math.Abs(p.Y / p.len())) / Math.PI;
                    if (p.Y > 0)
                    {
                         quater += 180;
                         if (p.X > 0)
                         {
                              quater += 180;
                              angle = -1 * angle;
                         }
                    }
                    else
                    {
                         if (p.X < 0)
                         {
                              quater += 180;
                              angle = -1 * angle;
                         }
                    }

                    return quater + angle;
               }

          }
          public static Double len(this Point vector)
          {
               return Math.Sqrt((vector.X * vector.X) + (vector.Y * vector.Y));
          }
          public static Double dist(this Point a, Point b)
          {
               Point c = a.sub(b);
               return c.len();
          }

     }
     #endregion
     public class LandmarkList : List<Landmark>
     {
          public bool SetAtId(int id, Landmark p)
          {
               for (int i = 0; i < this.Count; i++)
                    if (id == this[i].id)
                    {
                         this[i] = p;
                         return true;
                    }
               return false;
          }
          public bool contains(int id)
          {
               for (int i = 0; i < this.Count; i++)
                    if (id == this[i].id)
                         return true;
               return false;
          }
          public int getIdx(int id)
          {
               for (int i = 0; i < this.Count; i++)
                    if (id == this[i].id)
                         return i;
               return -1;
          }
     }
}
